#ifndef _REBO_REPLAN_FSM_H_
#define _REBO_REPLAN_FSM_H_

#include <Eigen/Eigen>
#include <algorithm>
#include <iostream>
#include <nav_msgs/Path.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/Imu.h>
#include <std_msgs/UInt8.h>
#include <std_msgs/Bool.h>
#include <ros/ros.h>
#include <std_msgs/Empty.h>
#include <vector>
#include <visualization_msgs/Marker.h>

#include <bspline_opt/bspline_optimizer.h>
#include <plan_env/grid_map.h>
#include <ego_planner/Bspline.h>
#include <ego_planner/DataDisp.h>
#include <plan_manage/planner_manager.h>
#include <traj_utils/planning_visualization.h>

#include <tf/transform_broadcaster.h>
#include "tf/transform_datatypes.h"
#include "ego_planner/PoseAdjust.h"

#include "forklift_msgs/DirectionPath.h"
#include "forklift_msgs/PathSequence.h"
#include "planning_msgs/ControlBack.h"
#include "geometry_msgs/Twist.h"

using std::vector;

namespace ego_planner
{
  struct DispatchData {
    Eigen::Vector3d end_pt;
    double end_yaw; // radins
    std::string name;
    bool can_bypass;
  };

// 定义TimerCounter类用于通用时间计数管理
class TimerCounter {
  private:
    bool is_counting_;            // 是否正在计时
    ros::Time start_time_;        // 开始计时的时间
    ros::Duration total_duration_; // 总计时时长
    std::string counter_name_;    // 计时器名称（用于日志）
    double report_interval_;      // 报告间隔（秒）
    ros::Time last_report_time_;  // 上次报告时间
    
  public:
    // 构造函数
    TimerCounter(const std::string& name = "")
      : is_counting_(false), total_duration_(0.0), counter_name_(name) {}
    
    // 开始计时
    void start() {
      if (!is_counting_) {
        start_time_ = ros::Time::now();
        last_report_time_ = start_time_;
        is_counting_ = true;
        if (!counter_name_.empty()) {
          ROS_INFO_STREAM("[" << counter_name_ << "] Timer started");
        }
      }
    }
    
    // 停止计时
    void stop() {
      if (is_counting_) {
        ros::Time stop_time = ros::Time::now();
        ros::Duration duration = stop_time - start_time_;
        total_duration_ += duration;
        is_counting_ = false;
        if (!counter_name_.empty()) {
          ROS_INFO_STREAM("[" << counter_name_ << "] Timer stopped. Duration: " 
                          << duration.toSec() << " sec, Total: " << total_duration_.toSec() << " sec");
        }
      }
    }
    
    // 重置计时器
    void reset() {
      is_counting_ = false;
      total_duration_ = ros::Duration(0.0);
      if (!counter_name_.empty()) {
        ROS_INFO_STREAM("[" << counter_name_ << "] Timer reset");
      }
    }
    
    // 获取当前持续时间
    ros::Duration getDuration() const {
      if (is_counting_) {
        return (ros::Time::now() - start_time_);
      } else {
        return ros::Duration(0.0);
      }
    }
    
    // 获取总计时时长
    ros::Duration getTotalDuration() const {
      if (is_counting_) {
        return total_duration_ + (ros::Time::now() - start_time_);
      } else {
        return total_duration_;
      }
    }
    
    // 检查是否正在计时
    bool isCounting() const {
      return is_counting_;
    }
  };

  class EGOReplanFSM
  {

  private:
    /* ---------- flag ---------- */
    enum FSM_EXEC_STATE
    {
      INIT,
      WAIT_TARGET,
      ADJUST_POSE,
      GEN_NEW_TRAJ,
      REPLAN_TRAJ,
      EXEC_TRAJ,
      EMERGENCY_STOP
    };
    enum TARGET_TYPE
    {
      MANUAL_TARGET = 1,
      PRESET_TARGET = 2,
      REFENCE_PATH = 3
    };

    /* planning utils */
    EGOPlannerManager::Ptr planner_manager_;
    PlanningVisualization::Ptr visualization_;
    ego_planner::DataDisp data_disp_;

    /* parameters */
    int target_type_; // 1 mannual select, 2 hard code
    double no_replan_thresh_, replan_thresh_;
    double planning_horizen_, planning_horizen_time_;
    double emergency_time_;

    /* planning data */
    bool have_target_, have_odom_, have_new_target_;
    FSM_EXEC_STATE exec_state_,last_state_;
    int continously_called_times_{0};

    /* motion data */
    geometry_msgs::Twist cmd_vel;
    std_msgs::UInt8 is_adjust_pose;
    double w_adjust;
    tf::Quaternion quat;
    bool is_pos_update;
    enum DIRECTION {POSITIVE=0,NEGATIVE=1};
    DIRECTION dir;

    double roll,pitch,yaw,yaw_start,yaw_error_threshold,yaw_error;
    Eigen::Vector3d odom_pos_, odom_vel_, odom_acc_; // odometry state
    Eigen::Quaterniond odom_orient_;

    Eigen::Vector3d start_pt_, start_vel_, start_acc_, start_yaw_; // start state
    Eigen::Vector3d end_pt_, end_vel_;                                       // goal state
    Eigen::Vector3d local_target_pt_, local_target_vel_;                     // local target state
    int current_wp_;

    bool flag_escape_emergency_;

    /* ROS utils */
    ros::NodeHandle node_;
    ros::Timer exec_timer_, safety_timer_;
    ros::Subscriber waypoint_sub_, odom_sub_;
    ros::Publisher replan_pub_, new_pub_, bspline_pub_, data_disp_pub_,odom_adjust_pub_,dir_pub,stop_pub;

    /* kkcqh change */
    bool need_replan_ = false;
    ros::Subscriber need_replan_sub_, start_bypass_sub_, dispatch_sub_, traj_server_twist_sub_;
    ros::Publisher adjust_pose_pub_, control_back_pub_;
    double kk_target_yaw_, kk_end_yaw_;
    bool end_adjust = false;
    bool kk_emergency_stop_ = false;

    DispatchData current_dispatch_data_;
    DispatchData reserve_dispatch_data_;
    geometry_msgs::Twist traj_server_twist_data_;
    TimerCounter end_collision_check_timer_conter_;

    void NeedReplanCallback(const std_msgs::BoolConstPtr &msg);
    void RequestPathCallback(const forklift_msgs::PathSequence::ConstPtr &msg);
    void StartPathCallback(const forklift_msgs::PathSequence::ConstPtr &msg);
    void TrajServerTwistCallback(const geometry_msgs::Twist::ConstPtr &msg);
    
    /// int8 FINISH = 0 int8 RUNNING = 1 int8 ERROR = 2
    void PubAControlBack(int type);
    void IntoNextDispatch();
    bool UpdateNextGoal();

    /* helper functions */
    bool callReboundReplan(bool flag_use_poly_init, bool flag_randomPolyTraj); // front-end and back-end method
    bool callEmergencyStop(Eigen::Vector3d stop_pos);                          // front-end and back-end method
    bool planFromCurrentTraj();
    bool CheckPoseValid(double thre_angle=10.0);

    /* return value: std::pair< Times of the same state be continuously called, current continuously called state > */
    void changeFSMExecState(FSM_EXEC_STATE new_state, string pos_call);
    void changeDirection();
    void checkYawError();
    double calculateYawError(double yaw_cur,double yaw_target);
    std::pair<int, EGOReplanFSM::FSM_EXEC_STATE> timesOfConsecutiveStateCalls();
    void printFSMExecState();

    void getLocalTarget();
    void publishBspline();

    /* ROS functions */
    void execFSMCallback(const ros::TimerEvent &e);
    void checkCollisionCallback(const ros::TimerEvent &e);
    void odometryCallback(const nav_msgs::OdometryConstPtr &msg);
    void goal_callback(const geometry_msgs::PoseStamped::ConstPtr &msg);

    bool checkCollision();
    bool checkTrajCollision(LocalTrajData* info, double check_horizon = -1.0);

  public:
    EGOReplanFSM(/* args */)
    {
    }
    ~EGOReplanFSM()
    {
    }

    void init(ros::NodeHandle &nh);

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  };

} // namespace ego_planner

#endif
